#目前完成的内容有：电机Pwm控制，舵机PWM控制，前灯刹车灯控制，蜂鸣器控制，速度测试，电机反转

import machine
from machine import Pin, PWM, Timer
import time
import _thread
from radar import radar_use_LR, radar_use_HB
led_ping = Pin(2, Pin.OUT)
led_ping.value(1)
way_chang=Pin(13, Pin.OUT)
way_chang.value(0)
# 板载蓝色led
horn_pin = Pin(15, Pin.OUT)
# 蜂鸣器，Pin15阵脚
horn_pin.value(1)
led=Pin(12, Pin.OUT)#前灯
led_back=Pin(11, Pin.OUT)#后灯
led_back.value(0)
led.value(0)

def horn():
    horn_pin.value(0)
    time.sleep(0.1)
    horn_pin.value(1)

pwm_sg90 = machine.PWM(Pin(4))
# 舵机，Pin4阵脚
pwm_sg90.freq(50)
pwm_sg90.duty(0)
def sg90(angle):
    duty = 2.5 + angle * 10 / 180
    pwm_sg90.duty(int(duty))
# 舵机角度控制函数

pwm_motor = machine.PWM(Pin(5))
# 电机，Pin5阵脚
pwm_motor.freq(50)
pwm_motor.duty(0)
def motor(run_speed):
    pwm_motor.duty(int(run_speed))
# 电机速度控制函数

Pin_speed_test = Pin(14, Pin.IN)
def speed_test():
    count = 0
    if Pin_speed_test.value() == 1:
        count += 1
    timer = Timer(1000, speed_test)
    timer.init(period=1000, callback=speed_test)
    speed_out = (count * 60)
    print('speed:', speed_out)
    return speed_out


# 速度测试函数

def led_erro():
    led.value(1)
    led_back.value(1)
    time.sleep(2)
    led.value(0)
    led_back.value(0)
    time.sleep(2)

def motor_back():
    motor_back = Pin(14, Pin.OUT)
    motor_back.value(1)
# 电机反转

def motor_stop():
    while True:
        motor_back()
        c=speed_test()
        kp = 0.6
        ki = 0.3
        kd = 0.6
        sigmaE = 0
        fs = 0
        umax = 100
        speed = c
        ebfore = c  # 更新上一次的偏差值
        sigmaE += sigmaE + c  # 计算积分部分值
        u = kp * c + ki * sigmaE + kd * (c - ebfore)  # 计算控制值
        if u > umax:
            u = umax
        if u < -umax:
            u = -umax
            # 最大控制量控制
        c = c + u - fs + ki * sigmaE  # 计算当前值
        motor(c)
        if c == 0:
            break
    
uart = machine.UART(0, baudrate=9600)  # 使用UART0，设置波特率为9600
uart.write('car ready')
data = uart.read(10)
print(data)
def modle_change():
    if uart.read() == 'manual control':
        return_out='manual '
        print('manual control')
        return return_out
    if uart.read() == 'auto control':
        return_out1='auto '
        print('auto control')
        return return_out1
def speed_out():
    uart.write(speed_test())
    print(speed_out())
#速度上传至遥控器端
def data_in():
    modle_change()
    uart.read(data_in())
    print(data_in())
    return data_in()
def online_ping():
    while True:
        uart.write('online')#在线状态上传
        print('online test ok')
def online_ping_back():
    while True:
        count=0
        time.sleep(1)
        count+=1
        if uart.read()=='Ping ok':
            count=0
            print('Ping back ok')
        else:
            return 'Ping back error'
def radar_distance(LR,HB):
    while True:
        radar_use_HB()
        radar_use_LR()
        if radar_use_LR >= LR:
            print('radar LR error')
            return 'radar LR error'
        if radar_use_HB >= HB:
            print('radar HB error')
            return 'radar HB error'
#激光雷达避障距离设定，LR为左右四个雷达，HB为前后两个雷达


while True:
    if data_in()=='manual':
        modle='manual'
        _thread.start_new_thread(online_ping, ())
        _thread.start_new_thread(speed_out, ())
        _thread.start_new_thread(data_in, ())
        #数据上传和下载
        data_control=data_in()
        def sg90_control():
            sg90(data_control(2))
            #方向舵机控制
        def motor_control():
            motor(data_control(1))
            #油门控制
        def way_control():
            if data_control(2) == 1:
                way_chang.value(1)
            else:break
            #方向控制
        _thread.start_new_thread(sg90_control, (data_in(4)))
        _thread.start_new_thread(motor_control, (data_in(2)))
        _thread.start_new_thread(way_control, (data_in(8)))
        #多线程控制

        if online_ping() == 'Ping back error':
            motor_stop()
            print('wait link,car stop!')
            led_erro()#灯光闪烁报警
            if online_ping() == 'Ping back ok':
                print('link ok,car run!')
                led.value(0)
                led_back.value(0)
    #在线检测
        def car_led():
            if data_in(2) != 0:
                led_back.value(0)
            #关闭刹车灯
            if data_in(10) == 1:
                motor_stop()
                led_back.value(1)
            #刹车的控制
            if data_in(12) == 0:
                led.value(0)
            else:
                led.value(1)
        #车灯控制

        _thread.start_new_thread(car_led, ())
    if data_in()=='auto':
        modle='auto'
        print('auto control')
        _thread.exit()
        